
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_baro_backend.c
  * @author     baiyang
  * @date       2021-11-21
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_baro.h"
#include "sensor_baro_backend.h"

#include <string.h>
#include <float.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
void baro_backend_destructor(sensor_baro_backend *baro_backend);
void baro_backend_update(sensor_baro_backend *baro_backend);
void baro_backend_accumulate(sensor_baro_backend *baro_backend);
void baro_backend_update_healthy_flag(sensor_baro_backend *baro_backend, uint8_t instance);

static void sensor_baro_publish_baro_raw(uint8_t i);
/*----------------------------------variable----------------------------------*/
// access to frontend
static sensor_baro *_frontend;

struct sensor_baro_backend_ops baro_backend_ops = {.sensor_baro_backend_destructor = baro_backend_destructor,
                                                        .update = baro_backend_update,
                                                        .accumulate = baro_backend_accumulate,
                                                        .update_healthy_flag = baro_backend_update_healthy_flag};

static const float FILTER_KOEF = 0.1f;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void baro_backend_destructor(sensor_baro_backend *baro_backend) { rt_mutex_delete(baro_backend->_mutex); }
void baro_backend_update(sensor_baro_backend *baro_backend) {}
void baro_backend_accumulate(sensor_baro_backend *baro_backend) {}

void sensor_baro_backend_ctor(sensor_baro_backend *baro_backend, char *name)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的指针变量初始为非零值。
    memset(baro_backend, 0, sizeof(sensor_baro_backend));

    _frontend = sensor_baro_get_singleton();

    baro_backend->ops = &baro_backend_ops;

    baro_backend->_mutex = rt_mutex_create(name, RT_IPC_FLAG_FIFO);
}

void baro_backend_update_healthy_flag(sensor_baro_backend *baro_backend, uint8_t instance)
{
    if (instance >= _frontend->_num_sensors) {
        return;
    }
    rt_mutex_take(baro_backend->_mutex, RT_WAITING_FOREVER);

    // consider a sensor as healthy if it has had an update in the
    // last 0.5 seconds and values are non-zero and have changed within the last 2 seconds
    const uint32_t now = time_millis();
    _frontend->sensors[instance].healthy =
        (now - _frontend->sensors[instance].last_update_ms < BARO_TIMEOUT_MS) &&
        (now - _frontend->sensors[instance].last_change_ms < BARO_DATA_CHANGE_TIMEOUT_MS) &&
        !math_flt_zero(_frontend->sensors[instance].pressure);

    if (_frontend->sensors[instance].temperature < -200 ||
        _frontend->sensors[instance].temperature > 200) {
        // if temperature is way out of range then we likely have bad
        // data from the sensor, treat is as unhealthy. This is done
        // so SPI sensors which have no data validity checking can
        // mark a sensor unhealthy
        _frontend->sensors[instance].healthy = false;
    }

    rt_mutex_release(baro_backend->_mutex);
}

/**
  * @brief       
  * @param[in]   baro_backend  
  * @param[in]   instance  
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_baro_backend_update(sensor_baro_backend *baro_backend, uint8_t instance)
{
    sensor_baro_backend_update2(baro_backend);
    sensor_baro_backend_update_healthy_flag(baro_backend, instance);
}

/*
  copy latest data to the frontend from a backend
 */
void sensor_baro_backend_copy_to_frontend(uint8_t instance, float pressure, float temperature)
{
    if (instance >= _frontend->_num_sensors) {
        return;
    }
    uint64_t now = time_micros64();

    // check for changes in data values
    if (!math_flt_equal(_frontend->sensors[instance].pressure, pressure) || !math_flt_equal(_frontend->sensors[instance].temperature, temperature)) {
        _frontend->sensors[instance].last_change_ms = now*0.001f;
    }

    // update readings
    _frontend->sensors[instance].pressure = pressure;
    _frontend->sensors[instance].temperature = temperature;
    _frontend->sensors[instance].last_update_ms = now*0.001f;
    _frontend->sensors[instance].last_update_us = now;

    sensor_baro_publish_baro_raw(instance);
}

/**
  * @brief       
  * @param[in]   baro_backend  
  * @param[in]   press  
  * @param[out]  
  * @retval      
  * @note        
  */
bool sensor_baro_backend_pressure_ok(sensor_baro_backend *baro_backend, float press)
{
    if (isinf(press) || isnan(press)) {
        return false;
    }

    const float range = (float)_frontend->_filter_range;
    if (range <= 0) {
        return true;
    }

    bool ret = true;
    if (math_flt_zero(baro_backend->_mean_pressure)) {
        baro_backend->_mean_pressure = press;
    } else {
        const float d = fabsf(baro_backend->_mean_pressure - press) / (baro_backend->_mean_pressure + press);  // diff divide by mean value in percent ( with the * 200.0f on later line)
        float koeff = FILTER_KOEF;

        if (d * 200.0f > range) {  // check the difference from mean value outside allowed range
            // printf("\nBaro pressure error: mean %f got %f\n", (double)_mean_pressure, (double)press );
            ret = false;
            koeff /= (d * 10.0f);  // 2.5 and more, so one bad sample never change mean more than 4%
            baro_backend->_error_count++;
        }
        baro_backend->_mean_pressure = baro_backend->_mean_pressure * (1 - koeff) + press * koeff; // complimentary filter 1/k
    }
    return ret;

}

// set bus ID of this instance, for BARO_DEVID parameters
void sensor_baro_backend_set_bus_id(uint8_t instance, uint32_t id)
{
    _frontend->sensors[instance].bus_id = (int32_t)id;
}

static void sensor_baro_publish_baro_raw(uint8_t i)
{
    uitc_sensor_baro_raw sensor_baro_raw;

    sensor_baro_raw.timestamp_us       = _frontend->sensors[i].last_update_us;
    sensor_baro_raw.healthy            = _frontend->sensors[i].healthy;
    sensor_baro_raw.temperature_deg    = _frontend->sensors[i].temperature;
    sensor_baro_raw.pressure_pa        = _frontend->sensors[i].pressure;
    sensor_baro_raw.ground_pressure_pa = _frontend->sensors[i].ground_pressure;

    if (i == 0) {
        itc_publish(ITC_ID(sensor_baro_raw), &sensor_baro_raw);
    } else if (i == 1) {
        itc_publish(ITC_ID(sensor_baro_raw2), &sensor_baro_raw);
    } else if (i == 2) {
        itc_publish(ITC_ID(sensor_baro_raw3), &sensor_baro_raw);
    }
}

/*------------------------------------test------------------------------------*/


